#include "smallsatos.h"
#include "can_tx.h"
#include "ECAN.h"

INT8U can_outgoing_queue_name[] = "Outgoing";
void* can_outgoing_queue_content[CAN_OUTGOING_QUEUE_SIZE];
OS_EVENT* can_outgoing_queue;

OS_EVENT* get_can_outgoing_queue(void)
{
	return can_outgoing_queue;
}

void can_tx(void* pdata)
{
	OS_CPU_SR  cpu_sr; /* CPU Status reg. backup (required for the critical mode) */
	INT16U err;
	can_param_t* param; /* Message to be transmitted */

    /* Initialise the outgoing queue */
	can_outgoing_queue = OSQCreate(&can_outgoing_queue_content, CAN_OUTGOING_QUEUE_SIZE);
	
	OSEventNameSet(can_outgoing_queue, can_outgoing_queue_name, &err);

	for (;;)
	{
	   /* Wait for a message */
		param = (can_param_t*)OSQPend(can_outgoing_queue, 0, &err);

		OS_ENTER_CRITICAL();
		LED0 = 1;
		MCP2551_RS = 0; /* Wake up the transceiver */
		while(!ECANSendMessage(param->address, param->data, param->length, ECAN_TX_PRIORITY_0 & 
				ECAN_TX_STD_FRAME & ECAN_TX_NO_RTR_FRAME));
		LED0 = 0;
		OS_EXIT_CRITICAL();

	}
}
